state_quaternion.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
#include "core/state_block/state_block.h"
#include "core/state_block/local_parametrization_quaternion.h"
 
namespace wolf
{
class StateQuaternion : public StateBlock
{
  public:
    StateQuaternion(bool _fixed = false, bool _transformable = true);
    StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true);
    StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true);
    ~StateQuaternion() override;
    static Eigen::VectorXd Identity();
    WOLF_STATE_BLOCK_CREATE(StateQuaternion);
 
    void            setIdentity(bool _notify = true) override;
    Eigen::VectorXd identity() const override;
 
    virtual void transform(const VectorComposite& _transformation) override;
};
 
inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable)
    : StateBlock("StateQuaternion",
                 _quaternion.coeffs(),
                 _fixed,
                 std::make_shared<LocalParametrizationQuaternionLocal>(),
                 _transformable)
{
    state_.normalize();
}
 
inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable)
    : StateBlock("StateQuaternion",
                 _state,
                 _fixed,
                 std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(),
                 _transformable)
{
    if (state_.size() != 4) throw std::runtime_error("The quaternion state vector must be of size 4!");
 
    state_.normalize();
}
 
inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable)
    : StateBlock("StateQuaternion",
                 4,
                 _fixed,
                 std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(),
                 _transformable)
{
    state_ = Eigen::Quaterniond::Identity().coeffs();
}
 
inline StateQuaternion::~StateQuaternion() {}
 
inline void StateQuaternion::setIdentity(bool _notify)
{
    setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
}
 
inline Eigen::VectorXd StateQuaternion::Identity()
{
    return Eigen::Quaterniond::Identity().coeffs();
}
 
inline Eigen::VectorXd StateQuaternion::identity() const
{
    return Eigen::Quaterniond::Identity().coeffs();
}
 
inline void StateQuaternion::transform(const VectorComposite& _transformation)
{
    if (transformable_)
        setState(
            (Eigen::Quaterniond(_transformation.at('O').data()) * Eigen::Quaterniond(getState().data())).coeffs());
}
 
}  // namespace wolf